﻿using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;
using System;
using System.Linq;

namespace cubeAdjustment
{
    internal class ElevationFeature
    {

        /// <summary>
        /// 权矩阵
        /// </summary>
        public Matrix<double> P;
        /// <summary>
        /// 协因数矩阵
        /// </summary>
        public Matrix<double> B;

        public Vector<double> l;

        public int r;

        internal bool NeedHuber = false;
        internal bool NeedIGG = false;
        internal bool NeedL1 = false;
        internal bool NeedLp = false ;
        internal bool NeedDenmark = false;
        internal bool NeedHample = false;
        internal bool NeedTukey = false;

        public Matrix<double> Qxx, Nbb, Qll;
        public Vector<double> W, X, V, H0, InitialElevation;

        public double Sigma0;
        bool Is1E6(double H0)
        {
            if (Math.Abs(H0 - 1e6) < 1)
            { return true; }
            else
            { return false; }
        }
        bool HaveZero(Vector<double> lists)
        {
            foreach (var item in lists)
            {
                if (Math.Abs(item - 1e6) < 1)
                { return true; }
            }
            return false;
        }

        public ElevationFeature(int UnknownNum, int Known, int[] KnownPoints, double[] KnownH, int[] StartPoint, int[] EndPoint, Vector DeltaH, Vector Distance, string Robust)
        {
            if (Robust.Contains("Huber"))
            {
                NeedHuber= true;
            }
            if (Robust.Contains("L1"))
            {
                NeedL1= true;
            }
            if (Robust.Contains("Lp"))
            {
                NeedLp= true;
            }
            if (Robust.Contains("IGG"))
            {
                NeedIGG = true;
            }
            if (Robust.Contains("Tukey"))
            {
                NeedTukey= true;
            }
            if (Robust.Contains("Denmark"))
            {
                NeedDenmark= true;
            }
            if (Robust.Contains("Hample"))
            {
                NeedHample = true; 
            }

            int Points = UnknownNum + Known;

            int[] PointType = new int[Points];
            int[] Unknown = new int[UnknownNum];

            this.InitialElevation = new DenseVector(Points);

            var n = StartPoint.Length;

            var Pt = 0;

            for (int i = 0; i < Points; i++)
            {
                if (KnownPoints.Contains(i))
                {
                    InitialElevation[i] = KnownH[Array.IndexOf(KnownPoints, i)];
                    PointType[i] = 1;
                }
                else
                {
                    InitialElevation[i] = 1e6;
                    Unknown[Pt] = i;
                    PointType[i] = 0;
                    Pt += 1;
                }
            }
            while (HaveZero(this.InitialElevation))
            {
                for (int i = 0; i < n; i++)
                {
                    int Start = StartPoint[i];
                    int End = EndPoint[i];
                    if (Is1E6(InitialElevation[Start]) && !Is1E6(InitialElevation[End]))
                    {
                        InitialElevation[Start] = InitialElevation[End] - DeltaH[i];
                    }
                    else if (!Is1E6(InitialElevation[Start]) && Is1E6(InitialElevation[End]))
                    {
                        InitialElevation[End] = InitialElevation[Start] + DeltaH[i];
                    }
                }
            }
            B = new DenseMatrix(n, UnknownNum);
            l = new DenseVector(n);
            V = new DenseVector(n);

            // Build B Matrix
            // Build L Vector
            for (int i = 0; i < n; i++)
            {
                int Start = StartPoint[i];
                int End = EndPoint[i];

                if (!KnownPoints.Contains(Start))
                {
                    for (int j = 0; j < UnknownNum; j++)
                    {
                        if (Unknown[j] == Start)
                            B[i, j] = -1;
                    }
                }
                if (!KnownPoints.Contains(End))
                {
                    for (int j = 0; j < UnknownNum; j++)
                    {
                        if (Unknown[j] == End)
                            B[i, j] = 1;
                    }
                }
                l[i] = InitialElevation[Start] - InitialElevation[End] + DeltaH[i];
            }


            if (NeedHuber)
            {
                // Iterate
                Matrix<double> P0;
                Vector<double> X0;
                P = RobustEstimatesFunc.Square(V);
                Nbb = B.Transpose() * P * B;
                W = B.Transpose() * P * l;
                X = Nbb.Inverse() * W;
                V = B * X - l;
                do
                {
                    P0 = P;
                    X0 = X;
                    Sigma0 = Math.Sqrt(V * P * V / r);
                    P = RobustEstimatesFunc.Huber(V, 2 * Sigma0);
                    Nbb = B.Transpose() * P * B;
                    W = B.Transpose() * P * l;
                    X = Nbb.Inverse() * W;
                    V = B * X - l;
                }
                while (!(DiagMatrix_Equal(P0, P) && Vector_Equal(X0, X)));
                H0 = V + DeltaH;
                r = n - UnknownNum;
                //Accuracy assessment part
                Sigma0 = Math.Sqrt(V * P * V / r);
                Qxx = Nbb.Inverse();
                Qll = B * Qxx * B.Transpose();
            }

            if (NeedHample)
            {
                // Iterate
                Matrix<double> P0;
                Vector<double> X0;
                P = RobustEstimatesFunc.Square(V);
                Nbb = B.Transpose() * P * B;
                W = B.Transpose() * P * l;
                X = Nbb.Inverse() * W;
                V = B * X - l;
                do
                {
                    P0 = P;
                    X0 = X;
                    Sigma0 = Math.Sqrt(V * P * V / r);
                    P = RobustEstimatesFunc.Hampel(V, Sigma0);
                    Nbb = B.Transpose() * P * B;
                    W = B.Transpose() * P * l;
                    X = Nbb.Inverse() * W;
                    V = B * X - l;
                }
                while (!(DiagMatrix_Equal(P0, P) && Vector_Equal(X0, X)));
                H0 = V + DeltaH;
                r = n - UnknownNum;
                //Accuracy assessment part
                Sigma0 = Math.Sqrt(V * P * V / r);
                Qxx = Nbb.Inverse();
                Qll = B * Qxx * B.Transpose();
            }

            if (NeedIGG)
            {
                // Iterate
                Matrix<double> P0;
                Vector<double> X0;
                P = RobustEstimatesFunc.Square(V);
                Nbb = B.Transpose() * P * B;
                W = B.Transpose() * P * l;
                X = Nbb.Inverse() * W;
                V = B * X - l;
                do
                {
                    P0 = P;
                    X0 = X;
                    Sigma0 = Math.Sqrt(V * P * V / r);
                    P = RobustEstimatesFunc.IGG(V, Sigma0);
                    Nbb = B.Transpose() * P * B;
                    W = B.Transpose() * P * l;
                    X = Nbb.Inverse() * W;
                    V = B * X - l;
                }
                while (!(DiagMatrix_Equal(P0, P) && Vector_Equal(X0, X)));
                H0 = V + DeltaH;
                r = n - UnknownNum;
                //Accuracy assessment part
                Sigma0 = Math.Sqrt(V * P * V / r);
                Qxx = Nbb.Inverse();
                Qll = B * Qxx * B.Transpose();
            }

            if (NeedTukey)
            {
                // Iterate
                Matrix<double> P0;
                Vector<double> X0;
                P = RobustEstimatesFunc.Square(V);
                Nbb = B.Transpose() * P * B;
                W = B.Transpose() * P * l;
                X = Nbb.Inverse() * W;
                V = B * X - l;
                do
                {
                    P0 = P;
                    X0 = X;
                    Sigma0 = Math.Sqrt(V * P * V / r);
                    P = RobustEstimatesFunc.Huber(V, 2 * Sigma0);
                    Nbb = B.Transpose() * P * B;
                    W = B.Transpose() * P * l;
                    X = Nbb.Inverse() * W;
                    V = B * X - l;
                }
                while (!(DiagMatrix_Equal(P0, P) && Vector_Equal(X0, X)));
                H0 = V + DeltaH;
                r = n - UnknownNum;
                //Accuracy assessment part
                Sigma0 = Math.Sqrt(V * P * V / r);
                Qxx = Nbb.Inverse();
                Qll = B * Qxx * B.Transpose();
            }

            if (NeedL1)
            {
                int i_times = 0;
                // Iterate
                Matrix<double> P0;
                Vector<double> X0;
                P = RobustEstimatesFunc.Square(V);
                Nbb = B.Transpose() * P * B;
                W = B.Transpose() * P * l;
                X = Nbb.Inverse() * W;
                V = B * X - l;
                do
                {
                    P0 = P;
                    X0 = X;
                    Sigma0 = Math.Sqrt(V * P * V / r);
                    P = RobustEstimatesFunc.L1(V);
                    Nbb = B.Transpose() * P * B;
                    W = B.Transpose() * P * l;
                    X = Nbb.Inverse() * W;
                    V = B * X - l;
                    i_times++;
                }
                while (!(DiagMatrix_Equal(P0, P) && Vector_Equal(X0, X) || i_times > 1000));
                H0 = V + DeltaH;
                r = n - UnknownNum;
                //Accuracy assessment part
                Sigma0 = Math.Sqrt(V * P * V / r);
                Qxx = Nbb.Inverse();
                Qll = B * Qxx * B.Transpose();
            }

            if (NeedLp)
            {
                // Iterate
                Matrix<double> P0;
                Vector<double> X0;
                P = RobustEstimatesFunc.Square(V);
                Nbb = B.Transpose() * P * B;
                W = B.Transpose() * P * l;
                X = Nbb.Inverse() * W;
                V = B * X - l;
                do
                {
                    P0 = P;
                    X0 = X;
                    Sigma0 = Math.Sqrt(V * P * V / r);
                    P = RobustEstimatesFunc.Lp(V, 1.5);
                    Nbb = B.Transpose() * P * B;
                    W = B.Transpose() * P * l;
                    X = Nbb.Inverse() * W;
                    V = B * X - l;
                }
                while (!(DiagMatrix_Equal(P0, P) && Vector_Equal(X0, X)));
                H0 = V + DeltaH;
                r = n - UnknownNum;
                //Accuracy assessment part
                Sigma0 = Math.Sqrt(V * P * V / r);
                Qxx = Nbb.Inverse();
                Qll = B * Qxx * B.Transpose();
            }

            if (NeedDenmark)
            {
                // Iterate
                Matrix<double> P0;
                Vector<double> X0;
                P = RobustEstimatesFunc.Square(V);
                Nbb = B.Transpose() * P * B;
                W = B.Transpose() * P * l;
                X = Nbb.Inverse() * W;
                V = B * X - l;
                do
                {
                    P0 = P;
                    X0 = X;
                    Sigma0 = Math.Sqrt(V * P * V / r);
                    P = RobustEstimatesFunc.Huber(V, 2 * Sigma0);
                    Nbb = B.Transpose() * P * B;
                    W = B.Transpose() * P * l;
                    X = Nbb.Inverse() * W;
                    V = B * X - l;
                }
                while (!(DiagMatrix_Equal(P0, P) && Vector_Equal(X0, X)));
                H0 = V + DeltaH;
                r = n - UnknownNum;
                //Accuracy assessment part
                Sigma0 = Math.Sqrt(V * P * V / r);
                Qxx = Nbb.Inverse();
                Qll = B * Qxx * B.Transpose();
            }
        }
        /// <summary>
        /// 计算向量是否相等
        /// </summary>
        /// <param name="vector1"></param>
        /// <param name="vector2"></param>
        /// <returns></returns>
        internal static bool Vector_Equal(Vector<double> vector1, Vector<double> vector2)
        {
            var n = vector1.Count;
            for (int i = 0; i < n; i++)
            {
                if (Math.Abs(vector1[i] - vector2[i]) > 1e-6)
                {
                    return false;
                }
            }
            return true;
        }

        /// <summary>
        /// 计算对角矩阵是否相等
        /// </summary>
        /// <param name="matrix1"></param>
        /// <param name="matrix2"></param>
        /// <returns></returns>
        internal static bool DiagMatrix_Equal(Matrix<double> matrix1, Matrix<double> matrix2)
        {
            var n = matrix1.ColumnCount;
            for (int i = 0; i < n; i++)
            {
                if (Math.Abs(matrix1[i, i] - matrix2[i, i]) > 1e-6)
                {
                    return false;
                }
            }
            return true;
        }
    }
}
